#include "user_sid27_service.h"

#include "uds_service.h"
#include "user_uds.h"

#include <stdint.h>
#include <stdlib.h>

static uint8_t org_seed_buf[4];
static uint8_t uds_fsa_cnt = 0;

static uint8_t rand_u8(void)
{
    static uint32_t cnt = 666;
    srand(rt_tick_get() + cnt);
    cnt++;

    return (rand() % 0xFF);
}

static uint32_t Key_algorithm(uint32_t seed)
{

    uint32_t STccKey = 0;

    uint32_t KeyResult = 0;

    KeyResult = ((((seed >> 1) ^ seed) << 3) ^ (seed >> 2));

    STccKey = KeyResult ^ KEY_K_CAN;

    return STccKey;
}

static int user_uds_security_access(uint8_t *key_buf, uint8_t *seed_buf)
{

    uint32_t key = 0, seed = 0, calc_result = 0;

#if (UDS_SEED_LEN == 2)

    key = (key_buf[0] << 8) | key_buf[1];

    seed = (seed_buf[0] << 8) | seed_buf[1];

    calc_result = Key_algorithm(seed);

    calc_result &= 0xFFFF;

#else

    key = (key_buf[0] << 24) | (key_buf[1] << 16) | (key_buf[2] << 8) | key_buf[3];

    seed = (seed_buf[0] << 24) | (seed_buf[1] << 16) | (seed_buf[2] << 8) | seed_buf[3];

    calc_result = Key_algorithm(seed);

#endif

    if (calc_result == key) {

        return 0;
    }

    else {

        // USER_UDS_LOG("result: %x %x %x\n", key, calc_result, seed);

        return 1;
    }
}

static int16_t uds_request_seed_callback(uint8_t *data)
{

    for (uint8_t i = 0; i < UDS_SEED_LEN; i++) {

        data[i] = rand_u8();
    }

    memcpy(org_seed_buf, data, UDS_SEED_LEN);

    return UDS_SEED_LEN;
}

static int16_t uds_send_key_callback(uint8_t *data)
{

    if (!user_uds_security_access(data, org_seed_buf)) {

        set_current_sa_lv(UDS_SA_LV1);

        return 0;
    }

    else {

        uds_fsa_cnt++;

        if (uds_fsa_cnt >= FAS_MAX_TIMES) {

            // 密钥尝试次数超过限值
            uds_timer_start(UDS_TIMER_FSA); // 锁定时间要求不能因模块断电被清零，这里暂未实现掉电保存的功能

            return -NRC_EXCEEDED_NUMBER_OF_ATTEMPTS;
        }

        else {

            // 密钥不匹配
            return -NRC_INVALID_KEY;
        }
    }
}

security_access_str user_sid27_table[] = {
    { REQUEST_SEED, UDS_SEED_LEN, uds_request_seed_callback },
    { SEND_KEY, UDS_KEY_LEN, uds_send_key_callback },
};

int16_t Security_Access_Port(uint8_t subfunction, uint8_t *p_data)
{

    int16_t ret;

    for (uint8_t index = 0; index < (sizeof(user_sid27_table) / sizeof(user_sid27_table[0])); index++) {

        if (subfunction == user_sid27_table[index].sub_func) {

            if (user_sid27_table[index].handle_cb != NULL) {

                ret = user_sid27_table[index].handle_cb(p_data);

                return ret;
            }
        }
    }

    ret = -NRC_SUBFUNCTION_NOT_SUPPORTED;

    return ret;
}
